#include <ros/ros.h>
#include <string>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

#include <angles/angles.h>
#include <cmath>
#include <vector>
#include <iostream>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>

#include <testlidar/map_builderV3.hpp>
#include <testlidar/tic_toc.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include "opencv2/opencv.hpp"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include "opencv2/opencv.hpp"

#include <cv_bridge/cv_bridge.h>

// #include <testlidar/cpoint_msgs.h>
#include <testlidar/cpoint_msgs.h>
#include <testlidar/rtkGPS.h>
#include <testlidar/rtkGPSmessage.h>
#include <testlidar/WGS84ToUTM.h>
#include <std_msgs/Bool.h>
// #define DEG2RAD (57.295779524)

#define DEG2RAD(x) ((x)*0.017453293)
ros::Publisher pubImgClickPoint;
tf::Vector3 tf1;
tf::Quaternion quat=tf::Quaternion(0.00, 0.00, 0.00, 1);
double Laititude, Longtitude, YawDeg;
Eigen::Matrix<double,3,3> TransferMatrix = Eigen::Matrix3d::Zero();
Eigen::Matrix<double, 3, 1> RemoteGPS;
Eigen::Matrix<double, 3, 1> result(0,0,0);
geometry_msgs::PointStamped laser_point,base_point;
Eigen::Vector4d odom_point;
Eigen::Vector3d point_vel;
void On_mouse(int event, int x, int y, int flags, void*)
{
    if (flags > 7 && flags < 16 && event == cv::EVENT_LBUTTONDOWN)
    {
//        previousPoint = Point(x, y);
        testlidar::cpoint_msgs msgs;
        msgs.x = x;
        msgs.y = y;
 
        pubImgClickPoint.publish(msgs);
        cout<<x<<","<<y<<endl;
    }
}



void GetClickedPointHandler(const geometry_msgs::PointStamped::ConstPtr& msg)
{
    if(msg->header.frame_id == "odom")
    {
    laser_point.point = msg->point;
    tf1=tf::Vector3(msg->point.x, msg->point.y, msg->point.z);
    Eigen::Vector3d point_2d;
    odom_point = Eigen::Vector4d(msg->point.x, msg->point.y, msg->point.z,1);

    }
    
    // // point_2d(0,0) = msg->point.x/(111000*cos(DEG2RAD(TransferMatrix(1,2))));
    // // point_2d(1,0) = msg->point.y/111000;
    // point_2d(0,0) = msg->point.x;
    // point_2d(1,0) = msg->point.y;
    // point_2d(2,0) = 1;
    // result = TransferMatrix.cast<double>() * point_2d;

    // cout<<"received a clicked point" << msg->point.x << "," << msg->point.y << "," << msg->point.z<<endl;
    
    // cout<<"calculate in WGS84"<<endl;
    // cout<<  setprecision(12) << result << endl;

    // // cout<< "error:" << setprecision(12) << (result - RemoteGPS)<<endl;
    // //     cout<< "error:" << setprecision(12) << (result - RemoteGPS).squaredNorm()<<endl;

    // // double LaiError = (result(1,0) - 30.2629191)*111000;
    // // double LonError = (result(0,0) - 120.1168912)*111000*cos(DEG2RAD(30.2629191));
    // // double LaiError = (result(0,0) - 222529.46871);//*111000;222608.649741
    // // double LonError = (result(1,0) - 3351417.85703);//*111000*cos(DEG2RAD(30.2629191));3351437.22778
    // double LaiError = (result(0,0) - 222608.649741);//*111000;222608.649741
    // double LonError = (result(1,0) - 3351437.22778);//*111000*cos(DEG2RAD(30.2629191));3351437.22778

    // double error = sqrt(LaiError*LaiError + LonError*LonError);
    // cout<<error<<endl;


}
void GetGPSHandler(const receive_gps::rtkGPS::ConstPtr& msg)
{
    TransferMatrix(0,2) = msg->north_meter;
    TransferMatrix(1,2) = msg->east_meter;
    TransferMatrix(2,2) = 1;
    TransferMatrix(0,0) = cos(DEG2RAD(-msg->yaw_rad - 180));
    TransferMatrix(0,1) = -sin(DEG2RAD(-msg->yaw_rad - 180));
    TransferMatrix(1,0) = sin(DEG2RAD(-msg->yaw_rad - 180));
    TransferMatrix(1,1) = cos(DEG2RAD(-msg->yaw_rad - 180));
    //cout<<TransferMatrix<<endl;
    // double nm = msg->north_meter;
    Laititude = msg->north_meter;
    Longtitude = msg->east_meter;
    YawDeg = msg->yaw_rad;

    point_vel = TransferMatrix.inverse().cast<double>() * result;

}

void GetRemoteGPSHandler(const receive_gps::rtkGPS::ConstPtr& msg)
{

    RemoteGPS(0,0) = msg->east_meter;
    RemoteGPS(1,0) = msg->north_meter;
    RemoteGPS(2,0) = 1;
    // double nm = msg->north_meter;

//    cout<< setprecision(12) << msg->north_meter << " " <<  msg->east_meter << " "<< setprecision(12) << msg->yaw_rad<<endl;
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "testlidar");
    ros::NodeHandle nh;
    ros::Subscriber ClickedPointSub,GetGPSSub,GetRemoteGPSSub;
    uint8_t ImgErrorFLag;
    double Lai, Lon;
    ros::param::get("~Laititude", Lai);
    ros::param::get("~Lontitude", Lon);
    cout<<Lai << " " << Lon<<endl;
    ClickedPointSub = nh.subscribe<geometry_msgs::PointStamped>
                        ("/clicked_point", 100, GetClickedPointHandler);
    GetGPSSub = nh.subscribe<receive_gps::rtkGPS>  
                        ("/rtkGPS_data", 100, GetGPSHandler);
    GetRemoteGPSSub = nh.subscribe<receive_gps::rtkGPS>  
                        ("/remoteGPS_data", 100, GetRemoteGPSHandler);
    pubImgClickPoint = nh.advertise<testlidar::cpoint_msgs>
                        ("/img_point", 1);
    ros::Rate rate(100);

    tf::Transform transform;
    tf::TransformBroadcaster broadcaster;

    cv::VideoCapture vcap;
    cv::Mat image;
    const std::string videoStreamAddress = "http://192.168.1.2:8080/?action=stream";
    vcap.open(videoStreamAddress);
    // if(!vcap.open(videoStreamAddress)){
    //     std::cout<<"Error opening video stream"<<endl;
        ImgErrorFLag = 1;
    // }
    // else
    // {
    //     vcap >> image;
    //     cv::imshow("img",image);
    //     cv::waitKey(1);
    //     cv::setMouseCallback ("img", On_mouse, 0);
    // }
    while(ros::ok())
    {
        if(ImgErrorFLag == 0)
        {
            vcap >> image;
            cv::imshow("img",image);
            cv::waitKey(1);
        }

        tf::StampedTransform tf_base2velodyne;
        tf::TransformListener tf_listener;
        tf_listener.waitForTransform("/odom", "/velodyne", ros::Time(0), ros::Duration(100.0));
        try
        {
            tf_listener.lookupTransform("/odom", "/velodyne", ros::Time(0), tf_base2velodyne);        

            tf::Transform tf_test(tf::Quaternion(0.00, 0.00, 0.00, 1), tf1);
            tf::Transform tf_result = tf_base2velodyne.inverse() * tf_test;
            cout<< "tf_result:"<< tf_result.getOrigin().x() << "  " << tf_result.getOrigin().y() << " " << tf_result.getOrigin().z()<<endl;


            Eigen::Vector3d point_2d = Eigen::Vector3d(tf_result.getOrigin().x(), tf_result.getOrigin().y(), 1);



            result = TransferMatrix.cast<double>() * point_2d;

            cout<<"RESULT"<<endl<<setprecision(12)<< result<<endl;
            
            double LaiError = (result(0,0) - Lai);//222608.649741);//*111000;222608.649741
            double LonError = (result(1,0) - Lon);//3351437.22778);//*111000*cos(DEG2RAD(30.2629191));3351437.22778

            double error = sqrt(LaiError*LaiError + LonError*LonError);
            cout<<error<<endl;
            

            
            tf::Transform vel2point(tf::Quaternion(0.00, 0.00, 0.00, 1), tf::Vector3(point_vel(0,0), point_vel(1,0), tf1.getZ()));
            tf::Transform odom2point = tf_base2velodyne * vel2point;

            broadcaster.sendTransform(
            tf::StampedTransform(odom2point,
            ros::Time::now(),"odom", "target"));

        }
        catch (exception e)
        {
            /* code for Catch */
        }
        

        ros::spinOnce();
        rate.sleep();
    }
    return 0; 
}